#ifndef BLDCMotor_h
#define BLDCMotor_h
#include "stdint.h"
#include "foc_utils.h"
#include "FOCMotor.h"
#include "BLDCDriver.h"
#include "Simple_PID.h"
#include "Simple_CurrentSense.h"
#include "Simple_Sensor.h"
#include "lowpass_filter.h"

#define modulation_centered  1    //!< flag (1) centered modulation around driver limit /2  or  (0) pulled to 0
/// 电机物理参数
typedef struct 
{
    float phase_resistance;    //motor phase resistance
    int   pole_pairs;          //motor pole pairs number
    float KV_rating;           //motor KV rating
    float phase_inductance;    //motor phase inductance
}Motor_physical_t;
/// 限制参数
typedef struct 
{
    float voltage_limit;      //电压限制
    float current_limit;      //电流限制
    float velocity_limit;     //速度限制
}Global_Limit_t;

typedef struct 
{
    // configuration structures
    TorqueControlType torque_controller; //!< parameter determining the torque control type
    MotionControlType controller; //!< parameter determining the control loop to be used
    FOCModulationType foc_modulation;//!<  parameter determining modulation algorithm
    float voltage_sensor_align;    //!< sensor and motor align voltage parameter
    float velocity_index_search;   //!< target velocity for index search 
}Motor_Config_t;
/// 
typedef struct 
{
    float               sensor_offset;          //!< user defined sensor zero offset
    float               zero_electric_angle;    //!< absolute zero electric angle - if available
    int                 sensor_direction;       //!< if sensor_direction == Direction::CCW then direction will be flipped to CW
}BLDC_Sensor_t;

/// 三相输出
typedef struct 
{
    float               Ua;
    float               Ub;
    float               Uc;
    float               Ualpha;
    float               Ubeta;
}PhaseVoltage_S;


typedef struct 
{
    float               target; //!< current target value - depends of the controller  
  	float               shaft_angle;//!< current motor angle             当前电机轴角度
  	float               electrical_angle;//!< current electrical angle   当前电角度
  	float               shaft_velocity;//!< current motor velocity       当前电机轴速度
    float               current_sp;//!< target current ( q current )     目标电流
    float               shaft_velocity_sp;//!< current target velocity   电机轴目标速度
    float               shaft_angle_sp;//!< current target angle         电机轴目标角度
    DQVoltage_s         voltage;//!< current d and q voltage set to the motor
    DQCurrent_s         current;//!< current d and q current measured
    PhaseVoltage_S      phase_pwm;
    float voltage_bemf; //!< estimated backemf voltage (if provided KV constant)
}BLDC_Run_t;
/// 
typedef struct
{
    Motor_physical_t          motor_physical;
    Global_Limit_t            motor_limit;
    Motor_Config_t            run_config;
    BLDCDriver_t              *driver;
    FOCMotorStatus            motor_status;    //!< motor status
    SimplePID_t               PID_current_q;   //parameter determining the q current PID config
    SimplePID_t               PID_current_d;   //parameter determining the d current PID config
    SimplePID_t               PID_velocity;    //
    SimplePID_t               P_angle;         //
	LowPassFilter_t           LPF_velocity;
	LowPassFilter_t           LPF_angle; 
    LowPassFilter_t           LPF_current_q;//!<  parameter determining the current Low pass filter configuration 
    LowPassFilter_t           LPF_current_d;//!<  parameter determining the current Low pass filter configuration 	
    Simple_CurrentSense_t     *current_sense;
    int8_t                    enabled;        //!< enabled or disabled motor flag
    BLDC_Sensor_t             sensor_config; 
    BLDC_Run_t                run_state;
    SimpleSensor_t            *sensor; 
    long                      open_loop_timestamp;
    /* data */
}BLDCMotor_t;

void BLDCMotor_Init(BLDCMotor_t *handle,int pp, float _R, float _KV, float _inductance);

void linkDriver(BLDCMotor_t *handle,BLDCDriver_t* _driver);
void linkSensor(BLDCMotor_t *handle,SimpleSensor_t *_sensor);  //

void  simple_loopFOC(BLDCMotor_t *handle);
int alignSensor(BLDCMotor_t *handle);
void setPhaseVoltage(BLDCMotor_t *handle, float Uq, float Ud, float angle_el);
void simple_move(BLDCMotor_t *handle,float new_target);     //

void Simple_init(BLDCMotor_t *handle);
#endif
